/*
 * COPYRIGHT (C) 2010-2011
 * RE2, INC.
 *
 * Author: AM
 *
 * This file is a part of darm_example_ik, which was developed for the
 * DARPA ARM ROBOT Track C project.
 * 
 * darm_example_ik is free software:
 * you can redistribute it and/or modify it under the terms of the GNU General
 * Public License as published by the Free Software Foundation, either version
 * 3 of the License, or (at your option) any later version.
 *
 * You should have received a copy of the GNU General Public License along
 * with this package.  If not, see <http://www.gnu.org/licenses/>.
 *
 * RE2, INC. disclaims all warranties with regard to this software, including
 * all implied warranties of merchantability and fitness, in no event shall
 * RE2, INC. be liable for any special, indirect or consequential damages or
 * any damages whatsoever resulting from loss of use, data or profits, whether
 * in an action of contract, negligence or other tortious action, arising out
 * of or in connection with the use or performance of this software.
 *
 */

#ifndef RE2_DARM_EXAMPLE_IK_H
#define RE2_DARM_EXAMPLE_IK_H

#include <ros/ros.h>

// message to support the kinematics solver
#include <kdl_parser/kdl_parser.hpp> 
#include <kinematics_msgs/GetPositionFK.h>
#include <kinematics_msgs/GetPositionIK.h>

// message includes for DARPA ARM messages
#include <WAM_msgs/ArmPositions.h>
#include <WAM_msgs/ArmProperties.h>
/* #include <WAM_msgs/ArmPositionProperties.h> */
/* #include <WAM_msgs/ArmVelocityProperties.h> */
/* #include <WAM_msgs/ArmTorqueProperties.h> */
#include <WAM_msgs/WAMEnums.h> 
#include <WAM_msgs/SetJointPositions.h>
#include <WAM_msgs/SetMode.h>

/* #include <BHand_msgs/GotoHomePosition.h> */
/* #include <BHand_msgs/BHandEnums.h> */
#include <BHand_msgs/BHandPositions.h>
/* #include <BHand_msgs/BHandProperties.h> */
/* #include <BHand_msgs/BHandVelocities.h> */
 #include <BHand_msgs/BHandStrainGauge.h>

#include <DPPanTilt_msgs/ReportDynamicInfo.h>
/* #include <DPPanTilt_msgs/ReportStaticInfo.h> */

#include <TestExecution_msgs/StartTest.h>
#include <TestExecution_msgs/StopTest.h>

#include <gazebo/ModelStates.h>
#include <gazebo/ModelState.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>

#include <geometry_msgs/QuaternionStamped.h>

// shared pointers, and mutex locking
#include <boost/function.hpp>
#include <boost/thread.hpp>

using std::string;

enum {RE2_LAST_CALL_IK = 1,
      RE2_LAST_CALL_FK};

const float RE2_LEFTSIDE  =  1.;
const float RE2_RIGHTSIDE = -1.;

class darmExampleIK
{
protected:
  ros::NodeHandle m_nodeHandle;
  urdf::Model m_robotModel;
  
  float m_solveSide;  // which side of the robot we are running with

  // arm pos and mode publishers
  ros::Publisher m_posPub;
  ros::Publisher m_modePub;

  // service clients for connecting to arm_kinematics
  ros::ServiceClient m_clientIK;
  ros::ServiceClient m_clientFK;

  // hand publishers
  ros::Publisher m_velPubHand;
  ros::Publisher m_posPubHand;
  ros::Publisher m_modePubHand;
  ros::Publisher m_homePubHand;

  // hand subscribers
  ros::Subscriber m_posHandSub;
  void posHandCallback(const BHand_msgs::BHandPositionsConstPtr& msg);
  // mutex to make sure data is valid
  boost::mutex m_posHandSubMutex;
  std::vector<float> m_currentPositionsBHand;

  ros::Subscriber m_strainHandSub;
  void strainHandCallback(const BHand_msgs::BHandStrainGaugeConstPtr& msg);
  // mutex to make sure data is valid
  boost::mutex m_strainHandSubMutex;
  std::vector<uint> m_currentStrainsBHand;


  
  // getting arm properties from WAM_Server
  ros::Subscriber m_propsSub;
  void propsCallback(const WAM_msgs::ArmPropertiesConstPtr& msg);
  wamMode m_wamMode;

  // getting arm positions from WAM_Server
  ros::Subscriber m_posSub;
  void posCallback(const WAM_msgs::ArmPositionsConstPtr& msg);
  WAM_msgs::SetJointPositions m_currentPositionsWAM;
  // mutex to make sure data is valid
  boost::mutex m_posSubMutex;

  // Head subscribers and publishers
  ros::Subscriber m_reportUpperDynamicInfoSubscriber;  // upper pan/tilt pos and vel
  ros::Subscriber m_reportLowerDynamicInfoSubscriber;  // lower pan/tilt pos and vel
  void upperDynamicInfoCallback(const DPPanTilt_msgs::ReportDynamicInfoConstPtr& msg );
  void lowerDynamicInfoCallback(const DPPanTilt_msgs::ReportDynamicInfoConstPtr& msg );
  // mutex to make sure data is valid
  boost::mutex m_headSubMutex;
  std::vector<float> m_upperDynamicData;
  std::vector<float> m_lowerDynamicData;

  ros::Publisher m_setUpperAbsolutePositionPanPublisher;  // upper pan and tilt publishers
  ros::Publisher m_setUpperAbsolutePositionTiltPublisher;
  ros::Publisher m_startUpperPositionControlPublisher;         // set pan/tilt head to position control
  ros::Publisher m_setLowerAbsolutePositionPanPublisher;  // lower pan and tilt publishers
  ros::Publisher m_setLowerAbsolutePositionTiltPublisher;
  ros::Publisher m_startLowerPositionControlPublisher;         // set pan/tilt head to position control


  // names/angles of joints and links from the robot model
  std::vector<std::string> m_jointNames;
  std::vector<std::string> m_linkNames;
  geometry_msgs::Pose      m_robotPose;  // xyz,quaternion

  // messages for doing forward and inverse kinematics
  kinematics_msgs::GetPositionIK::Request  m_requestIK;
  kinematics_msgs::GetPositionIK::Response m_responseIK;
  kinematics_msgs::GetPositionFK::Request  m_requestFK;
  kinematics_msgs::GetPositionFK::Response m_responseFK;

  // hand convenience functions
  void sendPosHand(const std::vector<float>& pos, bool wait = true, float epsilon = 2000);
  void sendVelHand(const std::vector<float>& vel);
  void stopVelHand();
  void homeHand();
  void modeActivatedHand(int index);

  // head convenience funtions
  void sendPosHead(const std::vector<float>& lowerPos, const std::vector<float>& upperPos, bool wait = true, double epsilon = 0.01);

  // functions to get inverse and forward kinematics 
  int getIK(std::vector<double>&jointAngles, const geometry_msgs::Pose& pose);
  int getFK(geometry_msgs::Pose& pose, const std::vector<double>&jointAngles);

  // function to move arm to xyzRPY, if wait == true, then don't return until within epsilon of nextPose, nextPose actually contains xyzRPY, not xyz,quaternion
  // default epsilon of 0.03rad, which is slightly more than 1.5degrees, which covers the range of final errors
  // teams have seen for trapezoidal moves
  bool moveArmIK(geometry_msgs::Pose& nextPose, bool wait = true, double epsilon = 0.03);

  bool moveArmFK(const std::vector<float>&jointAngles, bool wait = true, double epsilon = 0.03);

  // handle start/stop messages from dashboard
  ros::Publisher                    m_reportTestCompletePublisher;
  ros::Subscriber                   m_startTestSubscriber;
  ros::Subscriber                   m_stopTestSubscriber;
  void startTestCallback(const TestExecution_msgs::StartTestConstPtr& msg );
  void  stopTestCallback(const TestExecution_msgs::StopTestConstPtr& msg );

  bool m_runningCupDemo;
  boost::thread * m_cupThread;

  // get model states to find location of object in world
  ros::Subscriber                   m_modelstatesSubscriber;
  void                              modelStatesCallback(const gazebo::ModelStatesConstPtr& msg );
  bool                              m_modelStateValid;
  geometry_msgs::Pose               m_cupPoseInRobotFrame;
  tf::TransformListener             m_tfListener;
  
  // getting object position from object recognition node
  ros::Subscriber                   m_objectRecognizerSub;
  void objectRecognizerCallback(const geometry_msgs::QuaternionStampedConstPtr& msg);
  // mutex to make sure data is valid
  boost::mutex                      m_objectRecognizerMutex;
  geometry_msgs::Pose               m_objectPoseInRobotFrame;
  bool                              m_objectStateValid;

  void testVelocityPath();

public:

  darmExampleIK():m_runningCupDemo(false)
  {
  }
  
  // initialization function, must be explicitly called 
  int init();

  ~darmExampleIK()   
  { 
    m_posSub.shutdown();
    m_posHandSub.shutdown();
    m_reportUpperDynamicInfoSubscriber.shutdown();
    m_reportLowerDynamicInfoSubscriber.shutdown();
    m_startTestSubscriber.shutdown();
    m_stopTestSubscriber.shutdown();
  }

  // function that reads the keyboard to call IK and FK solvers
  void keyboardLoop();

  // run through script to pick up coffee mug
  void pickUpCup();
  void pickUpBall();
};


#endif
